#include<orbExt.hpp>
void computeORB(const cv::Mat &img, vector<KeyPoint> &keypoints, vector<DescType> &descriptors)
{
    const int half_patch_size = 8;
    const int half_boundary = 32;

    int badPoints = 0;
    for (auto &kp : keypoints)
    {
        if (kp.pt.x < half_boundary || kp.pt.y < half_boundary || (kp.pt.x >= img.cols - half_boundary) || kp.pt.y >= img.rows - half_boundary)
        {
            // remove the bad points that lay outside of the boundary
            badPoints++;
            descriptors.push_back({});
            continue;
        }
        float m01 = 0, m10 = 0;
        for (int dx = -half_patch_size; dx < half_patch_size; ++dx)
        {
            for (int dy = -half_patch_size; dy < half_patch_size; ++dy)
            {
                uchar pixel = img.at<uchar>(kp.pt.y + dy, kp.pt.x + dx);
                m01 += dx * pixel;
                m10 += dy * pixel;
            }
        }
        float m = sqrt(m01 * m01 + m10 * m10);
        float sin = m01 / m;
        float cos = m10 / m;

        DescType desc(8, 0);
        for (int i = 0; i < 8; i++)
        {
            uint32_t d = 0; // 每一个是8位 ，四个八位就是256位
            for (int k = 0; k < 32; k++)
            {
                int idx_pq = i * 8 + k;
                Point2f p(bit_pattern_31_[idx_pq * 4], bit_pattern_31_[idx_pq * 4 + 1]);
                Point2f q(bit_pattern_31_[idx_pq * 4 + 2], bit_pattern_31_[idx_pq * 4 + 3]);

                Point2f pp = Point2f(cos * p.x - sin * p.y, sin * p.x + cos * p.y) + kp.pt;
                Point2f qq = Point2f(cos * q.x - sin * q.y, sin * q.x + cos * q.y) + kp.pt;

                if (img.at<uchar>(int(pp.y), int(pp.x)) < img.at<uchar>(int(qq.y), int(qq.x)))
                {
                    d |= 1 << k;
                }
            }
            desc[i] = d;
        }
        descriptors.push_back(desc);
    }

    cout << "badpoints" << badPoints << "/" << keypoints.size() << endl;
}
void BfMatc(const vector<DescType> desc1, const vector<DescType> desc2, vector<cv::DMatch> &matches)
{
    for (size_t i1 = 0; i1 < desc1.size(); ++i1)
    {
        if (desc1[i1].empty())
            continue;
        DMatch m{i1, 0, 256};
        for (size_t i2 = 0; i2 < desc2.size(); ++i2)
        {
            if (desc2[i2].empty())
                continue;
            int distance = 0;
            for (int k = 0; k < 8; k++)
            {
                uint32_t x = desc1[i1][k];
                uint32_t y = desc2[i2][k];
                while (x != y)
                {
                    distance += (x & 1) ^ (y & 1);
                    x >>= 1;
                    y >>= 1;
                }
            }
            if (distance < d_max && distance < m.distance)
            {
                m.distance = distance;
                m.trainIdx = i2;
            }
        }
        if (m.distance < d_max)
        {
            matches.push_back(m);
        }
    }
    cout << matches.size() << endl;
}